// done
#include "car.hpp"
#include "help.hpp"
#include "utils/cmdline.hpp"
#include <iostream>
#include <random>
#include <vector>
#include <string>
#include <map>
#include <algorithm>
#include <cmath>
#include <unordered_map>
#include <ctime>
using namespace std;
/**
 * @description: 得到一个随机绿灯时间
 * @param {void}
 * @return {void}
 * @src: car.hpp
 */
int getRandomGreen()
{
  srand(time(0));
  int res = rand() % 60;
  return res;
}
/**
 * @description: initializer
 * @param {
 * Car& emergency, 
 * Car& team
 * }
 * @return {void}
 * @src: car.hpp
 */
void initializer(Car& emergency, Car& team)
{
  double l1 = team.getCurDistance();
  double v1 = team.getCurSpeed();
  double l0 = emergency.getCurDistance();
  double v0 = emergency.getCurSpeed();
  // 设置冲突时间
  // cout << l1 << ' ' << l0 << ' ' << v1 << ' ' << v0 << '\n';
  double t1 = (l1-l0) / (v1-v0);
  emergency.setTime_t1(t1);
  team.setTime_t1(t1);
  // 设置车列消散时间
  double td = l1 / v1;
  emergency.setTime_td(td);
  team.setTime_td(td);
  // 设置绿灯时间
  double tg = static_cast<double>(getRandomGreen());
  emergency.setTime_tg(tg);
  team.setTime_tg(tg);
  // 设置初始加速度
  emergency.setAccelerated(0);
  team.setAccelerated(0);
}
/**
 * @description: 车列消散时间小于绿灯剩余时间时的调整方法
 * @param {
 * emergency: 应急车辆
 * team： 排队车列
 * }
 * @return {void}
 * @src: trans
 */
void dissipationLessRemaining(Car& emergency, Car& team)
{
  // 此时车列消散时间小于绿灯剩余时间
  // a 、t的重新计算
  double td = emergency.getCurTime_td();
  double tg = emergency.getCurTime_tg();
  double t1 = emergency.getCurTime_t1();
  double l0 = emergency.getCurDistance();
  double v0 = emergency.getCurSpeed();
  double l1 = team.getCurDistance();
  double v1 = team.getCurSpeed();
  // sol 1: t1 < td, 
  // 即应急车辆与车列会发生冲突为避免冲突，须对应急车辆进行减速引导
  if (t1 <= td)
  {
    double a = pow((v1 - v0), 2.0);
    double b = 2 * (l1 - l0);
    double res = a / b;
    cout << "须对应急车辆进行减速引导，加速度应 <= " << fabs(res) << "m/s^2\n";
    LOG.write("须对应急车辆进行减速引导，加速度应 <= %f m/s^2", fabs(res));
  }
  // sol 2: t1 > td
  // 应急车辆与车列不发生冲突，前方车列可在剩余绿灯时间内消散，
  // 此时需要判断在剩余绿灯时间内，应急车辆能否通过交叉口
  else
  {
    // 应急车辆在剩余绿灯时间内可以顺利通过交叉口
    if (t1 < tg)
    {
      cout << "剩余绿灯时间可顺利通过，请保持原速通行\n";
      LOG.write("剩余绿灯时间可顺利通过，请保持原速通行");
    }
    // 应急车辆在剩余绿灯时间内不能通过交叉口，此时采取绿灯延时策略
    else 
    {
      double te = l0/v0 - tg;
      cout << "绿灯已延时 " << fabs(te) << "s, 请保持原速通行\n";
      LOG.write("绿灯已延时 %f s, 请保持原速通行", fabs(te));
    }
  }
}
/**
 * @description: 车列消散时间大于绿灯剩余时间时的调整方法
 * @param {
 * emergency: 应急车辆
 * team： 排队车列
 * }
 * @return {void}
 * @src: trans
 */
void dissipationGreaterRemaining(Car& emergency, Car& team)
{
  // 此时车列消散时间大于绿灯剩余时间
  // a 、t的重新计算
  double td = emergency.getCurTime_td();
  double tg = emergency.getCurTime_tg();
  double t1 = emergency.getCurTime_t1();
  double l0 = emergency.getCurDistance();
  double v0 = emergency.getCurSpeed();
  double l1 = team.getCurDistance();
  double v1 = team.getCurSpeed();
  // sol 1: t1 < td,
  // 应急车辆与车列会发生冲突，为避免冲突，应对应急车辆进行减速引导，采取绿灯延时策略
  if (t1 <= td)
  {
    double a = pow((v1 - v0), 2.0);
    double b = 2 * (l1 - l0);
    double a0 = a / b;
    cout << "须对应急车辆进行减速引导，加速度应 <= " << fabs(a0) << "m/s^2";
    LOG.write("须对应急车辆进行减速引导，加速度应 <= %f m/s^2", fabs(a0));
    double te = l1 / v1 - tg;
    cout << "同时需要对绿灯延时时长为: " << fabs(te) << "s\n";
    LOG.write("同时需要对绿灯延时时长为: %f s", fabs(te));
  }
  // sol 2: t1 > td
  // 应急车辆与车列不冲突，但在剩余绿灯时间内不能通过交叉口，此时采取绿灯延时策略
  {
    double te = l0 / v0 - tg;
    cout << "应急车辆保持原速通行，绿灯延时时间为: " << fabs(te) << "s\n";
    LOG.write("应急车辆保持原速通行，绿灯延时时间为: %f s", fabs(te));
  }
}
/**
 * @description: 当前交叉口为绿灯
 * @param {
 * Car& emergency, 
 * Car& team
 * }
 * @return {void}
 * @src: car.hpp
 */
void processGreen(Car& emergency, Car& team)
{
  cout << "当前交叉口为绿灯: \n";
  cout << "------------------------------------------------------------------------\n";
  double td = emergency.getCurTime_td();
  double tg = emergency.getCurTime_tg();
  double t1 = emergency.getCurTime_t1();
  // (1) 车列消散时间小于绿灯剩余时间即 td <= tg
  // sol: 根据 t1 来选择调整方法, 调用 dissipationLessRemaining 方法
  if (td <= tg)
  {
    dissipationLessRemaining(emergency, team);
  }
  // (2) 车列消散时间大于绿灯剩余时间即 td > tg
  // sol: 根据 t1 来选择调整方法, 调用 dissipationGreaterRemaining 方法
  else
  {
    dissipationGreaterRemaining(emergency, team);
  }
  cout << "------------------------------------------------------------------------\n";
}
/**
 * @description: 基于a,b,c,解一元二次方程
 * @param {
 * double a, 
 * double b, 
 * double c, 
 * double td
 * }
 * @return {void}
 * @src: process.cpp
 */
double calcQuadratic(double a, double b, double c, double td)
{
  double lamb = pow(b, 2.0) - 4 * a * c;
  if (lamb < 0) return td;
  double up1 = -1*b + sqrt(lamb);
  double up2 = -1*b - sqrt(lamb);
  double up = max(up1, up2);
  double res = up / 2*a;
  return res;
}
/**
 * @description: calcExtend
 * @param {
 * Car& emergency, 
 * Car& team
 * }
 * @return {void}
 * @src: process.cpp
 */
double calcExtend(Car& emergency, Car& team)
{
  double l0 = emergency.getCurDistance();
  double v0 = emergency.getCurSpeed();
  double l1 = team.getCurDistance();
  double v1 = team.getCurSpeed();
  double a0 = emergency.getAccelerated();
  double a1 = team.getAccelerated();
  // calc t0
  double t0 = v0 / (a1 - a0);
  // calc t2
  double up, down;
  up = pow(v0, 2.0) * l1 - 2 * a1 * pow((l0 - l1), 2.0);
  down = 2 * a1 * v0 * (l0 - l1);
  double t2 = up / down;
  // calc te
  double te;
  te = t0 + t2;
  return te;
}
/**
 * @description: 计算红灯时情况
 * @param {
 * Car& emergency
 * Car& team
 * }
 * @return {void}
 * @src: car.hpp
 */
void processRed(Car& emergency, Car& team)
{
  // 题取参数
  emergency.setTime_tg(0);
  team.setTime_tg(0);
  team.setAccelerated(3);
  team.setSpeed(0);
  double a1 = team.getAccelerated();
  double l0 = emergency.getCurDistance();
  double v0 = emergency.getCurSpeed();
  double l1 = team.getCurDistance();
  double v1 = team.getCurSpeed();
  // 先计算 td
  double td = sqrt(2*l1/a1);
  team.setTime_td(td);
  emergency.setTime_td(td);
  // 计算 t1
  double a = a1;
  double b = ((-2) * v0);
  double c = 2*(l0 - l1);
  double t1 = calcQuadratic(a, b, c, td);
  team.setTime_t1(t1);
  emergency.setTime_t1(t1);
  print(emergency, team);
  cout << "当前交叉口为红灯: \n";
  cout << "------------------------------------------------------------------------\n";
  // 分情况讨论
  // (1) 若 t1 >= td，即切断红灯、绿灯早启后，应急车辆与前方消散车列不发生冲突
  // 无须对应急车辆进行车速引导，使绿灯时长满足应急车辆通过即可
  if (t1 >= td)
  {
    double te = l0 / v0;
    cout << "应急车辆保持原速通行，绿灯延时时间为: " << fabs(te) << "s\n";
    LOG.write("应急车辆保持原速通行，绿灯延时时间为: %.2f s", fabs(te));
    //printf("应急车辆保持原速通行，绿灯延时时间为: %f s", te);
  }
  // (2) 若 t1 < td, 即切断红灯、绿灯早启后，应急车辆与前方消散车列发生冲突
  //     此时不仅要重新设置绿灯时长，还要对应急车辆进行减速引导
  else 
  {
    double axx = pow(v0, 2.0);
    double bxx = 2 * (l0 - l1);
    double a0 = axx / bxx;
    a0 = a1 - a0;
    emergency.setAccelerated(a0);
    cout << "须对应急车辆进行减速引导，加速度应 <= " << fabs(a0) << "m/s^2";
    LOG.write("须对应急车辆进行减速引导，加速度应 <= %f m/s^2", fabs(a0));
    double tg = calcExtend(emergency, team);
    cout << "同时需要对绿灯延时时长为: " << fabs(tg) << "s\n";
    LOG.write("同时需要对绿灯延时时长为: %f s", fabs(tg));
  }
  cout << "------------------------------------------------------------------------\n";
}